#!/bin/bash
#Created for reminding how the data was created
#1 Take rocla.txt (line map) and create 0.04 resolution map with reduced dimensions
./create_map rocla.txt 0 0 75

#copy outputs to unique name
cp out_result.jpg rocla_set_1_map.jpg
cp out-map.occu rocla-map.occu

#copy data from data repository (GIM/src/data) 
POSFILE="../../../../data/rocla_data/J2B2.MaCI_Position.Motion_22_06_2010_11:59:30.txt"
RANGFILE="../../../../data/rocla_data/J2B2.MaCI_Ranging.Laser_22_06_2010_11:59:30.txt"
#cp $POSFILE .
#cp $RANGFILE .
#Convert MaCI logfile to "LnMlib" logfile
./convert_maci_data -P $POSFILE -R $RANGFILE
#Store the measurement data
cp measurement-out.txt rocla_set_1.txt

#Set up the second log files
POSFILE2="../../../../data/rocla_data/J2B2.MaCI_Position.Motion_22_06_2010_12:16:09.txt"
RANGFILE2="../../../../data/rocla_data/J2B2.MaCI_Ranging.Laser_22_06_2010_12:16:09.txt"

#cp $POSFILE2 .
#cp $RANGFILE2 .
#Convert MaCI logfile to "LnMlib" logfile
./convert_maci_data -P $POSFILE2 -R $RANGFILE2
#Store the measurement data
cp measurement-out.txt rocla_set_2.txt

## Now we have set up a map and two data sets Have fun

# Matching the first dataset
#./scanmatch rocla_set_1.txt env_out_1.txt rocla_match_1.txt

#Matching the second dataset
#./scanmatch rocla_set_2.txt env_out_2.txt rocla_match_2.txt

## Next we need to match the reference set
## This set is "approximately matched" to the map, however the correleation in scanmatch error 
## affects to the quality of the matching

#./match2map rocla-map.occu rocla_match_1.txt 21.75 24.37 -3.45 map_matched_set1.txt 

## Next we will Match the data set to map, without adding the measurement to the map -
## This yields to poses which are correlated only with the map, howerer the matches are not that good
## The algorithm calculates "the goodness" of match by doing k-means 

#./match2map2 rocla-map.occu map_matched_set1.txt 21.75 24.67 -3.40 map2_matched_set1.txt

## Now the two above are Optimized using Least-squares.
## The result is stored to Global-Map.occu
# ./match_posesets

## Finally we will match the second set to matched sensor map.
# ./match2map-no-mapping Global-Map.occu rocla_match_2.txt 12.54 -16.59 2.067 map_matched_set2.txt

